#ifndef _AUTO_RUN_H_
#define _AUTO_RUN_H_

#include "MyServo.h"
#include "motor.h"

#define MIN_D           20      // 最小距离

MyServo  servo;                 // 舵机

inline void  auto_run (void);
inline float get_distance (void);
inline void  determine_dir (void);

inline void auto_run () {
    float s = get_distance ();

    if (s < MIN_D) {
        pause ();
        Serial.println ("too close, i need think about it!");
        delay (10);

        determine_dir ();
    } else {
        move_on ();
/*        
        switch (conf.state) {
            case SC_STOPPED :
                pause ();
                break;
            case SC_FORWARD :
                pwm_forward (&m0, conf.left_pwm);
                pwm_forward (&m1, conf.right_pwm);
                conf.state = SC_FORWARD;
                break;
        }
*/
    }
}

float get_distance () {
    uint32_t start = millis ();
    //给Trig发送一个低高低的短时间脉冲,触发测距  
    digitalWrite(TRIG, LOW); //给Trig发送一个低电平  
    delayMicroseconds(2);    //等待 2微妙  
    digitalWrite(TRIG, HIGH); //给Trig发送一个高电平  
    delayMicroseconds(10);    //等待 10微妙  
    digitalWrite(TRIG, LOW); //给Trig发送一个低电平  
    
    float temp = float(pulseIn(ECHO, HIGH)); //存储回波等待时间,  
    //pulseIn函数会等待引脚变为HIGH,开始计算时间,再等待变为LOW并停止计时  
    //返回脉冲的长度  
    
    //声速是:340m/1s 换算成 34000cm / 1000000μs => 34 / 1000  
    //因为发送到接收,实际是相同距离走了2回,所以要除以2  
    //距离(厘米)  =  (回波时间 * (34 / 1000)) / 2  
    //简化后的计算公式为 (回波时间 * 17)/ 1000  
    
    float cm = (temp * 17 )/1000; //把回波时间换算成cm  
    if (cm < 2) {
        delay (100);
        return get_distance ();
    }
    return cm;
}

inline void  determine_dir (void) {
    // check for left distance
    servo.write (180);
    float left = get_distance ();
/*
        Serial.print ("left distance = ");
        Serial.println (left);
*/

    // check for right distance
    servo.write (15);
    float right = get_distance ();
/*
        Serial.print ("right distance = ");
        Serial.println (right);
*/

    // reset the servo to center
    servo.write (90);

    if (left <= MIN_D && right <= MIN_D) {
        Serial.print ("neither left nor right has enough space, backward for ");
        Serial.print (conf.speed_time);

        backward ();
        delay (conf.speed_time);
        determine_dir ();
    } else {
        if (left > right) {
/*
            Serial.print ("left > right, turn left for ");
*/
            pwm_backward (&m0, conf.right_pwm);
            pwm_forward (&m1, conf.right_pwm);
/*
            Serial.print (conf.turn_left_time);
            Serial.println (" ms.");
*/
            delay (conf.turn_left_time);
        } else {
//                Serial.print ("left < right, trun right for ");
            pwm_forward (&m0, conf.right_pwm);
            pwm_backward (&m1, conf.right_pwm);
//                Serial.print (conf.turn_right_time);
//                Serial.println (" ms.");            
            delay (conf.turn_right_time);
        }
    }
    pause ();
    delay (500);
}
#endif // _AUTO_RUN_H_
